import time

from src.driver.lw100_qa_servo_motor.lw_qa_servo_motor_driver import LW100QAServoMotorDriver
from src.utils.decimal_conversion import DecimalConversion


class LWQAServoMotorAction:
    def __init__(self):
        self.motor_driver = LW100QAServoMotorDriver()
        self.driver_logger = self.motor_driver.logger

    def position_control_run(self, position_pulse: int):
        """
        位置控制电机运行
        :return:
        """
        # 选择位置控制模式
        self.motor_driver.set_control_mode(1)
        # 单向旋转运动，选择增量位置模式
        self.motor_driver.set_sport_mode(0)
        # 选择位置控制模式位置指令来源
        self.motor_driver.position_control_cmd_src(0)
        # 设置电机每旋转一圈的位置指令数
        self.motor_driver.position_control_pulse_count_percent_circle(position_pulse_number=50000)

        # 设置旋转方向
        self.motor_driver.set_rotate_direction(0)
        # 设置脉冲指令形态
        self.motor_driver.position_control_pulse_direction(0)

        # 电机使能
        # self.motor_driver.enable_motor_driver()

    def speed_control_run(self, speed_pulse: int):
        # 选择速度控制模式
        self.motor_driver.set_control_mode(0)

        # # 转换速度
        # if speed_pulse < 0:
        #     speed = DecimalConversion.signed_int_to_hex(speed_pulse)
        #     # speed = DecimalConversion.int_to_complement(speed_pulse, 2)
        #     self.driver_logger.info_log(f"speed value is: {speed}")
        #     int_speed = int(speed, 2)
        #     self.driver_logger.info_log(f"binary speed value is: {int_speed}")
        #     # 设置速度值
        #     self.motor_driver.speed_control_mode_speed_value(int_speed)
        # else:
        #     # 设置速度值
        #     self.motor_driver.speed_control_mode_speed_value(speed_pulse)
        self.motor_driver.speed_control_mode_speed_value(speed_pulse)
        # 设置速度到达阈值
        # self.motor_driver.speed_control_mode_speed_reach(speed_pulse)
        # 启动电机
        self.motor_driver.enable_motor_driver()

    def torque_control_run(self, torque_pulse: int):
        # 选择转矩控制模式
        self.motor_driver.set_control_mode(2)


if __name__ == '__main__':
    motor_action = LWQAServoMotorAction()
    # motor_action.position_control_run(position_pulse=100)
    motor_action.speed_control_run(speed_pulse=50)
